#include "SmurfsBSPNode.h"

BSPNode::BSPNode(void) { }
BSPNode::BSPNode(std::vector<SmurfsPolygon*> polys) {
	polygons = polys;
}
BSPNode::BSPNode(BSPNode* front, BSPNode* back, SmurfsPlane* plane) {
	frontChild = front;
	backChild = back;
	splitPlane = plane;
}
BSPNode::~BSPNode(void) { }

BSPNode* BSPNode::buildBSPTree(std::vector<SmurfsPolygon*>& polygons, int depth) { 
	// if there are no plolygons return null
	if(polygons.empty())
		return NULL;

	// get the number of polygons
	int numPolys = polygons.size();

	// if criterion for leaf is matched, create a leaf node from remaining polygons
	if(depth >= MAX_DEPTH || numPolys <= MIN_LEAF_SIZE) {
		return new BSPNode(polygons);
	}

	// select the best possible partitioning plane
	SmurfsPlane splitPlane = pickSplittingPlane(polygons);

	// create a front list and a back list for the partitioning
	std::vector<SmurfsPolygon*> frontList, backList;

	// Test each polygon against the split plane and add them to the appropriate list(s)
	for(int i = 0; i < numPolys; i++) {
		SmurfsPolygon* poly = polygons[i];
		SmurfsPolygon** frontPart = new SmurfsPolygon*[2];
		SmurfsPolygon** backPart = new SmurfsPolygon*[2];
		for(int i = 0; i < 2; i++) {
			frontPart[i] = new SmurfsPolygon();
			backPart[i] = new SmurfsPolygon();
		}

		int numFP, numBP;	// Number of polygons in front and in back
		numFP = numBP = 0;

		switch(classifyPolygonToPlane(poly, splitPlane)) {
		case COPLANAR_WITH_PLANE:
			// fall through to in front because in leaf-storing tree, coplanar
			//	polygons are sent to either side of the tree
		case IN_FRONT_OF_PLANE:
			frontList.push_back(poly);
			break;
		case BEHIND_PLANE:
			backList.push_back(poly);
			break;
		case STRADDLING_PLANE:
			// split the polygone and send a part to each side
			splitPolygon(*poly, splitPlane, frontPart, numFP, backPart, numBP);
			for(int i = 0; i < numFP; i++)
				frontList.push_back(frontPart[i]);
			for(int i = 0; i < numBP; i++)
				backList.push_back(backPart[i]);
			break;
		}
	}

	// Recursively build child subtrees and return new tree root combining them
	BSPNode* frontTree = buildBSPTree(frontList, depth + 1);
	BSPNode* backTree = buildBSPTree(backList, depth + 1);

	return new BSPNode(frontTree, backTree, &splitPlane); 
}
SmurfsPlane BSPNode::pickSplittingPlane(std::vector<SmurfsPolygon*>& polygons) { 
	// blend factor for optimizing (balance or splits)
	float k = 0.8f;

	// variables for tracing best splitting plane
	SmurfsPlane bestPlane;
	float bestScore = FLT_MAX;

	// try the plan of each polygon as a dividing plane
	for(unsigned int i = 0; i < polygons.size(); i++) {
		int numInFront, numBehind, numStraddling;
		numInFront = numBehind = numStraddling = 0;
		SmurfsPlane plane = SmurfsPlane::getPlaneFromPolygon(polygons[i]);

		// test against all other polygons
		for(unsigned int j = 0; j < polygons.size(); j++) {
			// ignore testing against self
			if(i == j) continue;

			// keep standing count of the various poly-plane relationships
			switch(classifyPolygonToPlane(polygons[j], plane)) {
			case COPLANAR_WITH_PLANE:
				// treated as being infront of plane so fall through
			case IN_FRONT_OF_PLANE:
				numInFront++;
				break;
			case BEHIND_PLANE:
				numBehind++;
				break;
			case STRADDLING_PLANE:
				numStraddling++;
				break;
			}
		}
		// compute score as a weighted combination
		// lowest score wins!
		float score = k * numStraddling + (1.0f - k) * abs(numInFront - numBehind);
		if(score < bestScore) {
			bestScore = score;
			bestPlane = plane;
		}
	}

	return bestPlane; 
}
void BSPNode::splitPolygon(SmurfsPolygon& polygon, SmurfsPlane splitPlane, 
	SmurfsPolygon** frontPoly, int& numFP, SmurfsPolygon** backPoly, int& numBP) {
	int numFront, numBack;
	numFront = numBack = 0;
	std::vector<Vector3> frontVerts;
	std::vector<Vector3> backVerts;
	
	// test all edges (a, b) starting with edge from last to first vertex
	int numVerts = 3;
	Vector3 a = polygon.getPoints()[numVerts - 1];
	PointClassification aSide = classifyPointToPlane(a, splitPlane);

	// loop over all the edges given by the vertex pair (n-1, n)
	for(int n = 0; n < numVerts; n++) {
		Vector3 b = polygon.getPoints()[n];
		PointClassification bSide = classifyPointToPlane(b, splitPlane);
		if(bSide == POINT_IN_FRONT_OF_PLANE) {
			if(aSide == POINT_BEHIND_PLANE) {
				// Edge (a, b) straddles, output intersection points to both areas
				Vector3 i;
				float intPer;
				SmurfsPlane::intersectEdgeAgainstPlane(a, b, splitPlane, intPer, i);
				//assert(classifyPointToPlane(i, splitPlane) == POINT_ON_PLANE);
				frontVerts.push_back(i);
				numFront++;
				backVerts.push_back(i);
				numBack++;
			}
			// in all three cases, output b to the front side
			frontVerts.push_back(b);
			numFront++;
		} else if(bSide == POINT_BEHIND_PLANE) {
			if(aSide == POINT_IN_FRONT_OF_PLANE) {
				// edge (a, b) straddles plane, output intersect point
				Vector3 i;
				float intPer;
				SmurfsPlane::intersectEdgeAgainstPlane(a, b, splitPlane, intPer, i);
				//assert(classifyPointToPlane(i, splitPlane) == POINT_ON_PLANE);
				frontVerts.push_back(i);
				numFront++;
				backVerts.push_back(i);
				numBack++;
			} else if(aSide == POINT_ON_PLANE) {
				// output a when edge a, b goes from 'on' to 'behind' plane
				backVerts.push_back(a);
				numBack++;
			}
			// in all three cases, output b to the back side
			backVerts.push_back(b);
			numBack++;
		} else {
			// b is on the plane. in all three cases output b to the front
			frontVerts.push_back(b);
			numFront++;

			// in one case also output b to the back
			if(aSide == POINT_BEHIND_PLANE) {
				backVerts.push_back(b);
				numBack++;
			}
		}
		// keep b as the sorting point of the next edge
		a = b;
		aSide = bSide;
	}


	// create and return two new polygons from the two vertex lists
	Vector3 verts[3];
	
	// Since we only deal with triangles, we can guarantee that a split polygon will 
	// have at most 4 vertices and that these vertices are in the same circular order
	// as standard polygons for this program. Therefore, we can split this into two
	// triangular polygons. 
	if(frontVerts.size() > 3) {
		numFP = 2;
		verts[0] = frontVerts[0]; verts[1] = frontVerts[1]; verts[2] = frontVerts[2];
		frontPoly[0] = new SmurfsPolygon(verts);
		verts[0] = frontVerts[2]; verts[1] = frontVerts[3]; verts[2] = frontVerts[0];
		frontPoly[1] = new SmurfsPolygon(verts);
	} else {
		numFP = 1;
		verts[0] = frontVerts[0]; verts[1] = frontVerts[1]; verts[2] = frontVerts[2];
		frontPoly[0] = new SmurfsPolygon(verts);
	}

	if(backVerts.size() > 3) {
		numBP = 2;
		verts[0] = backVerts[0]; verts[1] = backVerts[1]; verts[2] = backVerts[2];
		backPoly[0] = new SmurfsPolygon(verts);
		verts[0] = backVerts[2]; verts[1] = backVerts[3]; verts[2] = backVerts[0];
		backPoly[1] = new SmurfsPolygon(verts);
	} else {
		numBP = 1;
		verts[0] = backVerts[0]; verts[1] = backVerts[1]; verts[2] = backVerts[2];
		backPoly[0] = new SmurfsPolygon(verts);
	}
}

PointClassification BSPNode::classifyPointToPlane(Vector3 point, SmurfsPlane plane) { 
	// compute the signed distance of a point from a plane
	float dist = plane.getNormal()->dot(point) - plane.getDot();

	// classify point based on the signed distance
	if(dist > PLANE_THICKNESS_EPSILON)
		return POINT_IN_FRONT_OF_PLANE;
	if(dist < -PLANE_THICKNESS_EPSILON)
		return POINT_BEHIND_PLANE;
	return POINT_ON_PLANE;
}

PolygonClassification BSPNode::classifyPolygonToPlane(SmurfsPolygon* polygon, SmurfsPlane plane) { 
	// loop over all polygon vertices and count how many lie in front and how many lie behind
	int numInFront, numBehind;
	numInFront = numBehind = 0;
	int numVerts = 3;		// all polygons are triangles for this example -> might want to change to make it more versatile

	for(int i = 0; i < numVerts; i++) {
		Vector3 point = polygon->getPoints()[i];

		switch(classifyPointToPlane(point, plane)) {
		case POINT_IN_FRONT_OF_PLANE:
			numInFront++;
			break;
		case POINT_BEHIND_PLANE:
			numBehind++;
			break;
		}
	}

	// if vertices on both sides, it's straddling
	if(numBehind != 0 && numInFront != 0)
		return STRADDLING_PLANE;
	// if one or more vertices in front of plane and none behind, it's in front
	if(numInFront != 0)
		return IN_FRONT_OF_PLANE;
	// if one or more vertices are behind the plane and none in front, it's behind
	if(numBehind != 0)
		return BEHIND_PLANE;
	// all vertices lie on the plane so the polygon is coplanar
	return COPLANAR_WITH_PLANE; 
}